package com.sarc.games;

import java.util.Date;

import android.annotation.SuppressLint;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.opengl.Matrix;

public class SensorMathematics 
{

	float x_coor;
	float y_coor;
	float z_coor;
	float x_velocity;
	float y_velocity;
	float z_velocity;
	float x_acceler;
	float y_acceler;
	float z_acceler;
	float vec[] = new float[4];
	SensorManager sensorManager;
	Sensor accelerometer;
	float filterFactor;
	float[] gravity = new float[3];
	long secondCounter = 0;
	Date current;
	int xCountDown;
	int yCountDown;
	int zCountDown;
	int sign;
	float[] rotationMatrix;
	float[] initialRotation;
	boolean initRotate = false;
	
	
	public SensorMathematics(SensorManager _sensorManager){
		
		x_coor = 0;
		y_coor = 0;
		z_coor = 0;
		
		gravity[0] = 0;
		gravity[1] = 0;
		gravity[2] = 0;
		
		x_velocity = 0;
		y_velocity = 0;
		z_velocity = 0;
		vec[0] = 0;
		vec[1] = 0;
		vec[2] = 1;
		vec[3] = 1;
		sensorManager = _sensorManager;
		filterFactor = (float) 0.1;
		
		xCountDown = 0;
		yCountDown = 0;
		zCountDown = 0;
		sign = 1;
		rotationMatrix = new float[16];
		
		rotationMatrix[0] = 1;
		rotationMatrix[4] = 1;
		rotationMatrix[8] = 1;
		rotationMatrix[12] = 1;
		
		initialRotation = new float[16];
		initialRotation[0] = 1;
		initialRotation[1] = 0;
		initialRotation[2] = 0;
		initialRotation[3] = 0;
		initialRotation[4] = 0;
		initialRotation[5] = 1;
		initialRotation[6] = 0;
		initialRotation[7] = 0;
		initialRotation[8] = 0;
		initialRotation[9] = 0;
		initialRotation[10] = 1;
		initialRotation[11] = 0;
		initialRotation[12] = 0;
		initialRotation[13] = 0;
		initialRotation[14] = 0;
		initialRotation[15] = 1;
		
	}
	
	public void RecorderStart(/*final long period*/){
		
		sensorManager.registerListener(new SensorEventListener(){

			@Override
			public void onAccuracyChanged(Sensor arg0, int arg1) {
				// TODO Auto-generated method stub
				
			}

			@Override
			public void onSensorChanged(SensorEvent event) {
				// TODO Auto-generated method stub
				
				final float alpha = (float) 0.8;
				current = new Date();
				long period;
				if (secondCounter == 0) period = 0;
				else period = current.getTime() - secondCounter;
				secondCounter = current.getTime();
				for (int i =0; i < 3; i++)
					if (gravity[i] != 0) gravity[i] = alpha * gravity[i] + (1 - alpha) * event.values[i];
					else gravity[i] = event.values[i];


				x_acceler = x_acceler * (1 - filterFactor) + filterFactor * (event.values[0] - gravity[0]);
				y_acceler = y_acceler * (1 - filterFactor) + filterFactor * (event.values[1] - gravity[1]);
				z_acceler = z_acceler * (1 - filterFactor) + filterFactor * (event.values[2] - gravity[2]);
				
				if (xCountDown == 0 && x_acceler > 0.2){
					xCountDown = 40;
					sign = 1;
				}
				if (xCountDown == 0 && x_acceler < -0.2){
					xCountDown = 40;
					sign = -1;
				}
				
				if (xCountDown > 0){
					x_coor = x_coor + sign * StrictMath.abs(filterFactor *x_acceler * period / 1000);
					xCountDown--;
				}
				else x_coor = x_coor + filterFactor *x_acceler * period / 1000;
				
				if (yCountDown == 0 && y_acceler > 0.2){
					yCountDown = 40;
					sign = 1;
				}
				if (yCountDown == 0 && y_acceler < -0.2){
					yCountDown = 40;
					sign = -1;
				}
				
				if (yCountDown > 0){
					y_coor = y_coor + sign * StrictMath.abs(filterFactor *y_acceler * period / 1000);
					yCountDown--;
				}
				else y_coor = y_coor + filterFactor *y_acceler * period / 1000;
				
				if (zCountDown == 0 && z_acceler > 0.2){
					zCountDown = 40;
					sign = 1;
				}
				if (zCountDown == 0 && z_acceler < -0.2){
					zCountDown = 40;
					sign = -1;
				}
				
				if (zCountDown > 0){
					z_coor = z_coor + sign * StrictMath.abs(filterFactor *z_acceler * period / 1000);
					zCountDown--;
				}
				else z_coor = z_coor + filterFactor *z_acceler * period / 1000;
				
				
				x_velocity = x_velocity + x_acceler;
				x_velocity = x_velocity + x_acceler;
				x_velocity = x_velocity + x_acceler;
				
				
			}
			}, sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER), SensorManager.SENSOR_DELAY_GAME);
		
		
		
		sensorManager.registerListener(new SensorEventListener(){

			@Override
			public void onAccuracyChanged(Sensor arg0, int arg1) {
				// TODO Auto-generated method stub
				
			}

			@SuppressLint({ "NewApi", "NewApi" })
			@Override
			public void onSensorChanged(SensorEvent event) {
				// TODO Auto-generated method stub
				
				SensorManager.getRotationMatrixFromVector(rotationMatrix, event.values);
				
				if (!initRotate){
					initRotate = true;
					float[] tmpFloat = new float[16];
					Matrix.invertM(tmpFloat, 0, rotationMatrix, 0);
					initialRotation = tmpFloat;
				}
				
				float tmpvec[] = new float[4];
				tmpvec[0] = -rotationMatrix[2] + rotationMatrix[3];
				tmpvec[1] = -rotationMatrix[6] + rotationMatrix[7];
				tmpvec[2] = -rotationMatrix[10] + rotationMatrix[11];
				tmpvec[3] = -rotationMatrix[14] + rotationMatrix[15];
				
				vec[0] = tmpvec[0] * initialRotation[0] + tmpvec[1] * initialRotation[1] + tmpvec[2] * initialRotation[2] + tmpvec[3] * initialRotation[3];
				vec[1] = tmpvec[0] * initialRotation[4] + tmpvec[1] * initialRotation[5] + tmpvec[2] * initialRotation[6] + tmpvec[3] * initialRotation[7];
				vec[2] = tmpvec[0] * initialRotation[8] + tmpvec[1] * initialRotation[9] + tmpvec[2] * initialRotation[10] + tmpvec[3] * initialRotation[11];
				vec[3] = tmpvec[0] * initialRotation[12] + tmpvec[1] * initialRotation[13] + tmpvec[2] * initialRotation[14] + tmpvec[3] * initialRotation[15];
				
				
			}}, sensorManager.getDefaultSensor(Sensor.TYPE_ROTATION_VECTOR), SensorManager.SENSOR_DELAY_GAME);
		
		
		
	}
	
	float[] returnNormal (float[] p1, float[] p2, float[] p3){
		
		float[] p1p2 = new float[]{p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2]};
		float[] p1p3 = new float[]{p3[0]-p1[0], p3[1]-p1[1], p3[2]-p1[2]};
		
		float[] result = new float[]{p1p2[1]*p1p3[2]-p1p2[2]*p1p3[1], p1p2[2]*p1p3[0]-p1p2[0]*p1p3[2], p1p2[0]*p1p3[1]-p1p2[1]*p1p3[0]};
		float normalize = distance(result, new float[]{0, 0, 0});
		return new float[]{result[0] / normalize, result[1] / normalize, result[2] / normalize};
	}
	
	float[] returnIntersection (float[] vec, float[] p1, float[] p2, float[] p3){
		
		float[] normal = returnNormal(p1, p2, p3);
		float d = normal[0]*p1[0] + normal[1]*p1[1] + normal[2]*p1[2];
		float VN = vec[0]*normal[0] + vec[1]*normal[1] + vec[2]*normal[2];
		float t = d / VN;
		float[] intersection = new float[]{t*vec[0], t*vec[1], t*vec[2]};
		return intersection;
	}
	
	float isIntersect (float[] vec, float[] p1, float[] p2, float[] p3){
		
		float[] surfacePt = returnIntersection(vec, p1, p2, p3);
		float bigArea = triangleArea(p1, p2, p3);
		float a1 = triangleArea(surfacePt, p1, p2);
		float a2 = triangleArea(surfacePt, p2, p3);
		float a3 = triangleArea(surfacePt, p1, p3);
		float difference = Math.abs(bigArea - a1 - a2 - a3);
		return difference;
		
	}
	
	float distance (float[] p1, float[] p2){
		return (float) Math.sqrt((p2[0] - p1[0])*(p2[0] - p1[0]) + (p2[1] - p1[1])*(p2[1] - p1[1]) + (p2[2] - p1[2])*(p2[2] - p1[2]));
	}
	
	float triangleArea (float[] p1, float[] p2, float[] p3){
		float d1 = distance(p1, p2);
		float d2 = distance(p2, p3);
		float d3 = distance(p1, p3);
		return Float.parseFloat(Double.toString(Math.sqrt( (d1*d1 + d2*d2 + d3*d3) * (d1*d1 + d2*d2 + d3*d3) - 2 * ( Math.pow(d1, 4) + Math.pow(d2, 4) + Math.pow(d3, 4) ) ) / 4)) ;
	}
	
}
